/****************************************************************************
 *
 *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
 *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
 *           Julian Oes <joes@student.ethz.ch>
 *           luft27 <schn27@gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/* @file U-Blox protocol implementation */

#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <math.h>
#include <string.h>
#include <assert.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <drivers/drv_hrt.h>
#include <common/buffer.h>
#include <common/arraysize.h>

#include "ubx.h"

#define UBX_CONFIG_TIMEOUT 300

UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
fd_(fd),
gps_position_(gps_position),
timeout_(UBX_CONFIG_TIMEOUT)
{
}

UBX::~UBX()
{
}

int UBX::configure(unsigned &baudrate)
{
	const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200};

	timeout_ = UBX_CONFIG_TIMEOUT;

	for (unsigned int baud_i = 0; baud_i < ARRAYSIZE(baudrates); ++baud_i) {
		baudrate = baudrates[baud_i];
		set_baudrate(fd_, baudrate);

		Buffer b(buffer_, BUFFERSIZE);

		/* Send a CFG-PRT message to set the UBX protocol for in and out
		 * and leave the baudrate as it is, we just want an ACK-ACK from this
		 */

		/* Define the package contents, don't change the baudrate */
		b.setIndexForPut(0);
		b.putUint8(UBX_CFG_PRT_PAYLOAD_PORTID);
		b.putUint8(0);
		b.putUint16(0);
		b.putUint32(UBX_CFG_PRT_PAYLOAD_MODE);
		b.putUint32(baudrate);
		b.putUint16(UBX_CFG_PRT_PAYLOAD_INPROTOMASK);
		b.putUint16(UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK);
		b.putUint16(0);
		b.putUint16(0);
		if (!sendMsg(UBX_CLASS_CFG, UBX_MESSAGE_CFG_PRT, b.getIndexForPut(), buffer_))
			continue;

		/* Send a CFG-PRT message again, this time change the baudrate */
		b.setIndexForPut(0);
		b.putUint8(UBX_CFG_PRT_PAYLOAD_PORTID);
		b.putUint8(0);
		b.putUint16(0);
		b.putUint32(UBX_CFG_PRT_PAYLOAD_MODE);
		b.putUint32(UBX_CFG_PRT_PAYLOAD_BAUDRATE);
		b.putUint16(UBX_CFG_PRT_PAYLOAD_INPROTOMASK);
		b.putUint16(UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK);
		b.putUint16(0);
		b.putUint16(0);
		sendMsg(UBX_CLASS_CFG, UBX_MESSAGE_CFG_PRT, b.getIndexForPut(), buffer_);

		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
			set_baudrate(fd_, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
		}


		/* send a CFG-RATE message to define update rate */
		b.setIndexForPut(0);
		b.putUint16(UBX_CFG_RATE_PAYLOAD_MEASRATE);
		b.putUint16(UBX_CFG_RATE_PAYLOAD_NAVRATE);
		b.putUint16(UBX_CFG_RATE_PAYLOAD_TIMEREF);
		if (!sendMsg(UBX_CLASS_CFG, UBX_MESSAGE_CFG_RATE, b.getIndexForPut(), buffer_))
			continue;

		/* send a NAV5 message to set the options for the internal filter */
		b.setIndexForPut(0);
		b.putUint16(UBX_CFG_NAV5_PAYLOAD_MASK);
		b.putUint8(UBX_CFG_NAV5_PAYLOAD_DYNMODEL);
		b.putUint8(UBX_CFG_NAV5_PAYLOAD_FIXMODE);
		b.putUint32(0);
		b.putUint32(0);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint16(0);
		b.putUint16(0);
		b.putUint16(0);
		b.putUint16(0);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint32(0);
		b.putUint32(0);
		b.putUint32(0);
		if (!sendMsg(UBX_CLASS_CFG, UBX_MESSAGE_CFG_NAV5, b.getIndexForPut(), buffer_))
			continue;

		/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */

		b.setIndexForPut(0);
		b.putUint8(UBX_CLASS_NAV);
		b.putUint8(UBX_MESSAGE_NAV_POSLLH);
		b.putUint8(0);
		b.putUint8(UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		if (!sendMsg(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, b.getIndexForPut(), buffer_))
			continue;

		b.setIndexForPut(0);
		b.putUint8(UBX_CLASS_NAV);
		b.putUint8(UBX_MESSAGE_NAV_TIMEUTC);
		b.putUint8(0);
		b.putUint8(UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		if (!sendMsg(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, b.getIndexForPut(), buffer_))
			continue;

		b.setIndexForPut(0);
		b.putUint8(UBX_CLASS_NAV);
		b.putUint8(UBX_MESSAGE_NAV_SVINFO);
		b.putUint8(0);
		b.putUint8(UBX_CFG_MSG_PAYLOAD_RATE1_1HZ);		/* For satelites info 1Hz is enough */
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		if (!sendMsg(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, b.getIndexForPut(), buffer_))
			continue;

		b.setIndexForPut(0);
		b.putUint8(UBX_CLASS_NAV);
		b.putUint8(UBX_MESSAGE_NAV_SOL);
		b.putUint8(0);
		b.putUint8(UBX_CFG_MSG_PAYLOAD_RATE1_1HZ);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		if (!sendMsg(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, b.getIndexForPut(), buffer_))
			continue;

		b.setIndexForPut(0);
		b.putUint8(UBX_CLASS_NAV);
		b.putUint8(UBX_MESSAGE_NAV_VELNED);
		b.putUint8(0);
		b.putUint8(UBX_CFG_MSG_PAYLOAD_RATE1_1HZ);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		b.putUint8(0);
		if (!sendMsg(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, b.getIndexForPut(), buffer_))
			continue;

		return 0;
	}
	return -1;
}


int UBX::receive(unsigned timeout)
{
	timeout_ = timeout;

	uint8_t classId;
	uint8_t msgId;
	int payload = recvMsg(classId, msgId, BUFFERSIZE, buffer_);

	if (payload < 0)
		return 0;

	Buffer b(buffer_, payload);

	if (classId == UBX_CLASS_NAV && msgId == UBX_MESSAGE_NAV_POSLLH) {
		gps_position_->lon = (int32_t)b.getUint32(4);
		gps_position_->lat = (int32_t)b.getUint32(8);
		gps_position_->alt = (int32_t)b.getUint32(16);
		gps_position_->eph_m = b.getUint32(20) * 1e-3f; // from mm to m
		gps_position_->epv_m = b.getUint32(24) * 1e-3f; // from mm to m
		gps_position_->timestamp_position = hrt_absolute_time();

	} else if (classId == UBX_CLASS_NAV && msgId == UBX_MESSAGE_NAV_TIMEUTC) {
		//convert to unix timestamp
		tm timeinfo;
		timeinfo.tm_year = b.getUint16(12) - 1900;
		timeinfo.tm_mon = b.getUint8(14) - 1;
		timeinfo.tm_mday = b.getUint8(15);
		timeinfo.tm_hour = b.getUint8(16);
		timeinfo.tm_min = b.getUint8(17);
		timeinfo.tm_sec = b.getUint8(18);
		time_t epoch = mktime(&timeinfo);

		gps_position_->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
		gps_position_->time_gps_usec += (uint64_t)(b.getUint32(8) * 1e-3f);
		gps_position_->timestamp_time = hrt_absolute_time();

	} else if (classId == UBX_CLASS_NAV && msgId == UBX_MESSAGE_NAV_SVINFO) {
		uint8_t numCh = b.getUint8(4);
		if (numCh > 20)
			numCh = 20;

		uint8_t satellites_used = 0;
		int i;

		for (i = 0; i < numCh; ++i) {
			gps_position_->satellite_prn[i] = b.getUint8(9 + 12 * i);

			uint8_t flags = b.getUint8(10 + 12 * i);

			if (!(flags & (1 << 4))) {
				if (flags & 1) {
					gps_position_->satellite_used[i] = 1;
					++satellites_used;
				} else
					gps_position_->satellite_used[i] = 0;

				gps_position_->satellite_snr[i] = b.getUint8(12 + 12 * i);
				gps_position_->satellite_elevation[i] = b.getUint8(13 + 12 * i);
				gps_position_->satellite_azimuth[i] = (uint8_t)((int16_t)b.getUint16(14 + 12 * i) * 255.0f / 360.0f);

			} else {
				gps_position_->satellite_used[i] = 0;
				gps_position_->satellite_snr[i] = 0;
				gps_position_->satellite_elevation[i] = 0;
				gps_position_->satellite_azimuth[i] = 0;
			}
		}

		/* Unused channels have to be set to zero for e.g. MAVLink */
		for (i = numCh; i < 20; i++) {
			gps_position_->satellite_prn[i] = 0;
			gps_position_->satellite_used[i] = 0;
			gps_position_->satellite_snr[i] = 0;
			gps_position_->satellite_elevation[i] = 0;
			gps_position_->satellite_azimuth[i] = 0;
		}

		gps_position_->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
		gps_position_->satellite_info_available = numCh > 0;
		gps_position_->timestamp_satellites = hrt_absolute_time();

	} else if (classId == UBX_CLASS_NAV && msgId == UBX_MESSAGE_NAV_SOL) {
		gps_position_->fix_type = b.getUint8(10);
		gps_position_->s_variance_m_s = b.getUint32(40);
		gps_position_->p_variance_m = b.getUint32(24);
		gps_position_->timestamp_variance = hrt_absolute_time();

	} else if (classId == UBX_CLASS_NAV && msgId == UBX_MESSAGE_NAV_VELNED) {
		gps_position_->vel_n_m_s = (int32_t)b.getUint32(4) * 1e-2f;
		gps_position_->vel_e_m_s = (int32_t)b.getUint32(8) * 1e-2f;
		gps_position_->vel_d_m_s = (int32_t)b.getUint32(12) * 1e-2f;
		gps_position_->vel_m_s   = b.getUint32(16) * 1e-2f;
		gps_position_->cog_rad   = (int32_t)b.getUint32(24) * M_DEG_TO_RAD_F * 1e-5f;
		gps_position_->vel_ned_valid = true;
		gps_position_->timestamp_velocity = hrt_absolute_time();
	}

	return 1;
}


bool UBX::sendMsg(uint8_t classId, uint8_t msgId, uint16_t payloadSize, const uint8_t* payload, bool ack)
{
	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};

	uint8_t head[4];
	Buffer b(head, sizeof(head));
	b.putUint8(classId);
	b.putUint8(msgId);
	b.putUint16(payloadSize);

	uint8_t chk[2] = {0};
	addCheckSum(chk, head, sizeof(head));
	addCheckSum(chk, payload, payloadSize);

	int retry = 5;

	while (retry--) {
		write(fd_, sync_bytes, sizeof(sync_bytes));
		write(fd_, head, sizeof(head));
		write(fd_, payload, payloadSize);
		write(fd_, chk, sizeof(chk));

		if (!ack || recvAck(classId, msgId))
			return true;
	}

	return false;
}


int UBX::recvMsg(uint8_t &classId, uint8_t &msgId, uint16_t maxpayloadSize, uint8_t* payload)
{
	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
	unsigned i = 0;
	unsigned cnt = 0;

	while (i < sizeof(sync_bytes)) {
		uint8_t ch = 0;
		if (portread(&ch, 1) != 1)
			return -1;

		if (ch != sync_bytes[i++])
			i = (ch == sync_bytes[0]) ? 1 : 0;

		if (++cnt > 1000)
			return -1;
	}

	uint8_t chk[2] = {0};

	uint8_t head[4];
	Buffer b(head, sizeof(head));
	if (portread(head, sizeof(head)) != sizeof(head))
		return -1;

	addCheckSum(chk, head, sizeof(head));

	classId = b.getUint8(0);
	msgId = b.getUint8(1);

	int size = b.getUint16(2);

	if (size > maxpayloadSize)
		return -1;

	if (size > 0 && portread(payload, size) != size)
		return -1;

	addCheckSum(chk, payload, size);

	uint8_t rchk[2];
	if (portread(rchk, sizeof(rchk)) != sizeof(rchk))
		return -1;

	if (chk[0] != rchk[0] || chk[1] != rchk[1])
		return -1;

	return size;
}


bool UBX::recvAck(uint8_t classId, uint8_t msgId)
{
	uint8_t classId_ = 0;
	uint8_t msgId_ = 0;
	uint8_t buf[2] = {0};

	while (classId_ != UBX_CLASS_ACK) {
		if (recvMsg(classId_, msgId_, sizeof(buf), buf) < 0)
			return false;
	}

	return msgId_ == UBX_MESSAGE_ACK_ACK && buf[0] == classId && buf[1] == msgId;
}

void UBX::addCheckSum(uint8_t *sum, const uint8_t *buf, int n)
{
	for (int i = 0; i < n; ++i) {
		sum[0] += buf[i];
		sum[1] += sum[0];
	}
}


int UBX::portread(uint8_t* buf, int n)
{
	pollfd fds[1];
	memset(&fds[0], 0, sizeof(fds[0]));
	fds[0].fd = fd_;
	fds[0].events = POLLIN;

	int r = 0;

	while (::poll(fds, ARRAYSIZE(fds), timeout_) > 0) {
		if (fds[0].revents & POLLIN) {
			r += ::read(fd_, buf + r, n - r);
			if (r == n)
				return r;
		}
	}

	return r;
}

